package edu.wpi.first.wpilibj.templates;

/**
 * @author Victoria
 */
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 
public class SimpleDrive {

    private static final boolean DEBUGGING = true;
    
    //Victors and speeds
    private Victor leftMotor, rightMotor;
    private double commandedLeftMotorSpeed, commandedRightMotorSpeed;
    
    //Encoders for autonomous
//    private Encoder driveEncoderLeft;
//    private Encoder driveEncoderRight;
//    private CounterBase.EncodingType factor;

    //Drive joysticks
    private Joystick leftJoystick, rightJoystick;
    
    //Constants
    private static final double DEADBAND = 0.05; //Deadband values
//    private static final double AUTONOMOUS_SPEED = -0.5; //Autonomous drive speed
//    private static final double DISTANCE_TO_DRIVE = -1.95; //2.2 meters from bumper at front of key
//    
    //Constructor    
    public SimpleDrive(Joystick ldrivejoystick, Joystick rdrivejoystick, int leftMotorPort, int rightMotorPort) {
       
        //Initializing Victors
        leftMotor = new Victor(leftMotorPort);
        rightMotor = new Victor(rightMotorPort);
        
        //Initializing joysticks
        leftJoystick = ldrivejoystick;
        rightJoystick = rdrivejoystick;

        //Initializing encoders
//        factor = EncodingType.k1X;
//        driveEncoderLeft = new Encoder(leftEncoderA, leftEncoderB, true);//FIX ME
//        driveEncoderLeft.setDistancePerPulse(0.00278 * 0.25 * 1.1);//FIX ME?
//        driveEncoderRight = new Encoder(rightEncoderA, rightEncoderB, true);//FIX ME
//        driveEncoderRight.setDistancePerPulse(0.00278 * 0.25 * 1.1);//FIX ME?
//        driveEncoderLeft.reset();
//        driveEncoderRight.reset();
//        driveEncoderLeft.start();
//        driveEncoderRight.start();
//        driveEncoderRight.setDistancePerPulse(0.00278 * 0.25);//FIX ME?
    }

    //Left joystick controls speed
    //Right joystick controls turn rate
    public void arcadeDrive() {
        double speed = leftJoystick.getY();
        double turnRate = rightJoystick.getX();
        commandedLeftMotorSpeed = speed * (1 + turnRate);
        commandedRightMotorSpeed = speed * (1 - turnRate);
        driveWithDeadband(commandedLeftMotorSpeed, commandedRightMotorSpeed);
        
        //Prints commanded speed, commanded turn rate, and Victor speeds
        if (DEBUGGING) {
            System.out.println("speed: " + speed + " turnRate: " + turnRate + " L: " + commandedLeftMotorSpeed + " R: " + commandedRightMotorSpeed);
        }
    }

    //Left joystick controls left side
    //Right joystick controls right side
    public void tankDrive() {
        commandedLeftMotorSpeed = leftJoystick.getY();
        commandedRightMotorSpeed = rightJoystick.getY();
        driveWithDeadband(commandedLeftMotorSpeed, commandedRightMotorSpeed);
        if (DEBUGGING) {
            System.out.println("left: " + leftJoystick.getY() + " right: " + rightJoystick.getY());
        }
    }

    //Sets commanded speeds within deadband to 0
    //Not scaled
    private void driveWithDeadband(double leftSpeed, double rightSpeed) {
        if (leftSpeed < DEADBAND && leftSpeed > -DEADBAND) {
            setLeftMotors(0.0);
        } else {
            setLeftMotors(leftSpeed);
        }
        if (rightSpeed < DEADBAND && rightSpeed > -DEADBAND) {
            setRightMotors(0.0);
        } else {
            setRightMotors(rightSpeed);
        }
    }

//    public boolean auto() {
//        double distanceL = driveEncoderLeft.getDistance();
//        double distanceR = driveEncoderRight.getDistance();
        //System.out.println("encoderdistance: " + distanceL + " rate: " + driveEncoderLeft.getRate() + " motorL: " + leftMotorsA.get() + " R: " + rightMotorsA.get());
        //double speedL = ((((distanceL - DISTANCE_TO_DRIVE) / DISTANCE_TO_DRIVE) * .8) + 0.2); // play with these constants
        //double speedR = ((((distanceR - DISTANCE_TO_DRIVE) / DISTANCE_TO_DRIVE) * .8) + 0.2);

//        if (distanceL >= DISTANCE_TO_DRIVE) { //Driving backwards
//            leftMotorsA.set(AUTONOMOUS_SPEED);
//            leftMotorsB.set(AUTONOMOUS_SPEED);
//        }
//        if (distanceR <= DISTANCE_TO_DRIVE) { //FIXME (check the sign)
//            rightMotorsA.set(-AUTONOMOUS_SPEED);
//            rightMotorsB.set(-AUTONOMOUS_SPEED);
//        }
        
//        if (DEBUGGING){ System.out.println("encoderR: " + distanceR + " encoderL: " + distanceL);}
//
//        //Returns true when distance has been reached
//        if (distanceL <= DISTANCE_TO_DRIVE && distanceR >= DISTANCE_TO_DRIVE) { //Switched sign because going negative
//            this.stopMotors();
//            return true;
//        } else {
//            return false;
//        }
//    }

//    //Resets encoders
//    public void resettingEncoders() {
//        driveEncoderLeft.reset();
//        driveEncoderRight.reset();
//    }

    //Sets all motors to 0
    public void stopMotors() {
        leftMotor.set(0.0);
        rightMotor.set(0.0);
    }

    //Sets both left motors
    private void setLeftMotors(double speed) {
        leftMotor.set(speed);
        //leftMotorsB.set(speed);
    }

    //Sets both right motors
    private void setRightMotors(double speed) {
        // this joystick is backwards
        rightMotor.set(-speed);
        //rightMotorsB.set(-speed);
    }
}
